/*
云视窗类CloudViewer是简单显示点云的可视化工具类，可以让用户用尽可能少的代码查看点云。注意：点云视窗类不能应用于多线程应用程序中。
PCLVisualizer是PCL的全功能可视化类。虽然使用比CloudViewer更复杂，但功能更强大，可提供显示法线，绘图形状和多个视口等功能
*/


#include <pcl/common/common_headers.h>  
#include <pcl/io/pcd_io.h>                      //PCD文件读取

#include <pcl/visualization/pcl_visualizer.h> 
#include <pcl/visualization/cloud_viewer.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pcl/common/transforms.h>

#include <pcl/features/normal_3d_omp.h>
#include <omp.h>
using namespace std;
 
int main(int argc, char **argv) {//柱型点云测试
  setlocale(LC_ALL, "zh_CN.utf8");  // 设置中文
  cout << "Test PCL !" << endl;  
  // 智能指针 初始化一个XYZRGB类型的点云对象
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>); 
  pcl::PointCloud<pcl::PointXYZ>::Ptr pc2(new pcl::PointCloud<pcl::PointXYZ>);
  
  uint8_t r(255), g(15), b(15);  // 颜色值
  
  for (float z(-1.0); z <= 1.0; z += 0.05) //制作柱型点云集 z方向（-1,1）每格0.05单位
  {
  	for (float angle(0.0); angle <= 360.0; angle += 5.0) // 每个水平面
    {  // 角度，每5度
      pcl::PointXYZRGB point;
      pcl::PointXYZ xyz;

      xyz.x = cos (pcl::deg2rad(angle)); 
      xyz.y = sin (pcl::deg2rad(angle));
      xyz.z = z;

      // 计算水平上旋转5度的坐标
      point.x = cos (pcl::deg2rad(angle)); 
      point.y = sin (pcl::deg2rad(angle));
      point.z = z;
   
      uint32_t rgb = (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
      
      point.rgb = *reinterpret_cast<float*>(&rgb);
      point_cloud_ptr->points.push_back (point);  // 填充点云
      pc2->points.push_back(xyz);
    }
    if (z < 0.0) {//颜色渐变
      r -= 12;
      g += 12;
    }
    else {
      g -= 12;
      b += 12;
    }
  }
  
  point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
  point_cloud_ptr->height = 1;

  pc2->width = (int) point_cloud_ptr->points.size ();
  pc2->height = 1;

  Eigen::Affine3f transform = Eigen::Affine3f::Identity();
  cout << "Affine3f:\n" << transform.matrix() << endl;
  transform.rotate(Eigen::AngleAxisf(-1.57, Eigen::Vector3f::UnitX() ));

  // pcl::PointCloud<pcl::PointXYZRGB> pointcloud2;
  // pointcloud2 = *point_cloud_ptr;
  
  pcl::transformPointCloud(*point_cloud_ptr, *point_cloud_ptr, transform);

  cout << "Affine3f:\n" << transform.matrix() << endl;
  

  // pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;//创建法线估计向量
	// ne.setNumberOfThreads(omp_get_max_threads());
	// ne.setInputCloud(point_cloud_ptr);	
	// pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());//创建一个空的KdTree对象，并把它传递给法线估计向量，基于给出的输入数据集，KdTree将被建立
	// ne.setSearchMethod(tree);
	// ne.setViewPoint(10, 10, 10);//设置视点，官网上的pcl::flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Vector4f &normal)是对“一个已知点”的法线进行手动定向
	// //ne.setViewPoint(-10, -10, -10);
	// pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);//存储输出数据
	// //ne.setRadiusSearch(1);        //半径单位为毫米
	// ne.setKSearch(20);
	// ne.compute(*cloud_normals);

  //估计法线
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	ne.setInputCloud (pc2);
 
	//创建一个空的kdtree对象，并把它传递给法线估计对象
	//基于给出的输入数据集，kdtree将被建立   
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
	ne.setSearchMethod (tree);
	//输出数据集
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
	//使用半径在查询点周围3厘米范围内的所有邻元素
	ne.setRadiusSearch (10);  //半径单位为毫米
	//计算特征值
	ne.compute (*cloud_normals);
	// pcl::io::savePCDFileASCII("cloud_normals.pcd", *cloud_normals);
	// cloud_normals->points.size ()应该与input cloud_downsampled->points.size ()有相同尺寸
	std::cout << "Normal calculated successfully !" << std::endl;


  
  //法线可视化
	pcl::visualization::PCLVisualizer viewer1("PCL Viewer");
  // viewer1.setBackgroundColor (0.5, 0.2, 0.7);
	// viewer1.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(pc2, cloud_normals);
	std::cout << "cloud_with_normals display!" << std::endl;

  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> single_color(point_cloud_ptr, 0, 255, 0);
  viewer1.addPointCloud<pcl::PointXYZRGB>(point_cloud_ptr,single_color);
  
  // pcl::visualization::CloudViewer viewer ("pcltest");
  // viewer.showCloud(point_cloud_ptr); 
  // !viewer.wasStopped()


  while (  !viewer1.wasStopped())
  {
    viewer1.spinOnce ();
 };
  return 0;
}